#include "motiondetect.h"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <iostream>
#include <cstdio>
#include <cassert>
#include <cmath>
#include <cstdlib>

#include "timestamp.h"

using namespace std;
using namespace cv;
static inline void setRectRange(Rect &rect, int width, int height){
    if (rect.x < 0 ){
        rect.x = 0;
    }else if (rect.x > width - 1){
        rect.x = width - 1;
    }
    if (rect.y < 0){
        rect.y = 0;
    }else if (rect.y > height - 1){
        rect.y = height - 1;
    }
    if (rect.width <= 0){
        rect.width = 1;
    }else if(rect.x + rect.width > width){
        rect.width = width - rect.x;
    }
    if(rect.height <= 0){
        rect.height = 1;
    }else if(rect.height + rect.y > height){
        rect.height = height - rect.y;
    }
}

MotionDetect::MotionDetect()
{
    this->m_find_cost = 0;
    this->m_detect_cost = 0;
    this->m_transform_cost = 0;
    this->m_disp_cost = 0;
    this->m_keypoints_cost = 0;
    this->m_match_cost = 0;
    this->m_track_cost = 0;
    this->m_velo_x = 0;
    this->m_velo_y = 0;
    this->m_adjust_cost = 0;
    this->m_old_points.clear();
}

MotionDetect::~MotionDetect(){

}


bool MotionDetect::matchColor(Mat pre, Rect object, Mat &cur, RotatedRect found){

    return true;
}

bool MotionDetect::matchObject(Mat pre, Rect object, Mat &cur, Rect &found, bool use_old){
    //for measurement
    TimeStamp ts;
    //for sift
    SIFT sift;
    vector<KeyPoint> obj_keypoints;
    vector<KeyPoint> cur_keypoints;
    Mat obj_descriptors;
    Mat cur_descriptors;
    BFMatcher matcher(NORM_L1);
    vector<DMatch> matches;
    vector<DMatch> good_matches;

    //for match
    double min_dist = 100;
    double max_dist = 0;

    //for affine
    vector<Point2f> obj_points;
    vector<Point2f> found_points;
    if(use_old && this->m_old_keypoints.size() >= 3 && !this->m_old_descriptors.empty()){

    }else{
        Mat mask = Mat::zeros(pre.size(), CV_8U);
        mask(object).setTo(1);
        sift(pre, mask, obj_keypoints, obj_descriptors, false);
    }
    sift(cur, Mat::ones(cur.size(), CV_8U), cur_keypoints, cur_descriptors, false);

    matcher.match(obj_descriptors, cur_descriptors, matches, Mat());

    for (int i = 0; i < (int)matches.size(); i++){
        double dist = matches[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }

    for (int i = 0; i < (int)matches.size(); i++){
        if(matches[i].distance < 5 * min_dist){
            good_matches.push_back(matches[i]);
        }
    }

    Mat img_matches;
    drawMatches(pre, obj_keypoints, cur, cur_keypoints, good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
                vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

    imshow("match", img_matches);

    if(good_matches.size() < 3){
        return false;
    }

    for (int i = 0 ;i < (int)good_matches.size() ;i++){
        obj_points.push_back(obj_keypoints[good_matches[i].queryIdx].pt);
        found_points.push_back(cur_keypoints[good_matches[i].trainIdx].pt);
    }

    Mat obj_warp_mat;
    getDisplacement(obj_points, found_points, obj_points, found_points, obj_warp_mat, obj_warp_mat);

    found = object;
    found.x += obj_warp_mat.at<double>(0, 2);
    found.y += obj_warp_mat.at<double>(1, 2);
    double sin_theta = obj_warp_mat.at<double>(1, 0);
    double cos_theta = sqrt(1 - sin_theta * sin_theta);
    found.width *= (obj_warp_mat.at<double>(0, 0) / cos_theta);
    found.height *= (obj_warp_mat.at<double>(0, 0) / cos_theta);

    setRectRange(found, cur.cols, cur.rows);

    this->m_match_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool MotionDetect::getKeyPointsSURF(cv::Mat img, cv::Mat mask, std::vector<cv::KeyPoint> &keypoints){
     TimeStamp ts;
     SURF surf;
     surf(img, mask, keypoints, cv::noArray(), false);
     this->m_keypoints_cost = TimeStamp::now()->diff(ts);
     return true;
}

bool MotionDetect::getKeyPointsSIFT(cv::Mat img, cv::Mat mask, std::vector<cv::KeyPoint> &keypoints){
     TimeStamp ts;
     SIFT sift;
     sift(img, mask, keypoints, cv::noArray(), false);
     this->m_keypoints_cost = TimeStamp::now()->diff(ts);
     return true;
}

bool MotionDetect::findObject(Mat pre, Rect object, Mat &cur, Rect &found, bool use_old){
    //for measurement
    TimeStamp ts;

    //for features
    Mat gray;

    //for optical flow
    vector<Point2f> pre_points;
    vector<Point2f> cur_points;
    vector<unsigned char> status;
    vector<float> err;

    //for object
    vector<Point2f> pre_obj_points;
    vector<Point2f> obj_points;

    //for displacement
    vector<Point2f> pre_bg_points;
    vector<Point2f> bg_points;


    cvtColor(pre, gray, CV_BGR2GRAY);
    goodFeaturesToTrack(gray, pre_points, 1000, 0.1, 2);

    if(use_old){
        for (int i = 0; i < (int)this->m_old_points.size(); i++){
            pre_points.push_back(this->m_old_points[i]);
        }
    }
    if(pre_points.size() < 24){
        return false;
    }

    calcOpticalFlowPyrLK(pre, cur, pre_points, cur_points, status, err);

    Rect inner = object;
    inner.x = object.x + object.width / 4;
    inner.y = object.y + object.height / 4;
    inner.width /= 2;
    inner.height /= 2;

    for(int i = 0; i < (int) pre_points.size(); i++){
        if(status[i] > 0){
            if(testRect(object, pre_points[i])){
                if(testRect(inner, pre_points[i])){
                    pre_obj_points.push_back(pre_points[i]);
                    obj_points.push_back(cur_points[i]);
                    Scalar color(rand() & 255, rand() & 255, rand() & 255);
                    circle(cur, cur_points[i], 2, color);
                }
            }else{
                pre_bg_points.push_back(pre_points[i]);
                bg_points.push_back(cur_points[i]);
            }
        }
    }

    if(obj_points.size() < 3 || bg_points.size() < 3)
        return false;

    Mat obj_warp_mat, bg_warp_mat;

    if(!getDisplacement(pre_bg_points, bg_points, pre_obj_points, obj_points, bg_warp_mat, obj_warp_mat)){
        return false;
    }

    this->m_old_points = cur_points;

    found = object;
    found.x += obj_warp_mat.at<double>(0, 2);
    found.y += obj_warp_mat.at<double>(1, 2);
    double sin_theta = obj_warp_mat.at<double>(1, 0);
    double cos_theta = sqrt(1 - sin_theta * sin_theta);
    found.width *= (obj_warp_mat.at<double>(0, 0) / cos_theta);
    found.height *= (obj_warp_mat.at<double>(0, 0) / cos_theta);


    cout<<obj_warp_mat.col(2)<<"\n";



    this->m_find_cost = TimeStamp::now()->diff(ts);
    return true;
}


bool MotionDetect::findObjects(Mat pre, vector<Rect> objects, Mat cur, vector<Rect> &found, vector<unsigned char> &status){

}

bool MotionDetect::getDisplacement(vector<Point2f> bg_pre,
                                   vector<Point2f> bg_cur,
                                   vector<Point2f> fg_pre,
                                   vector<Point2f> fg_cur,
                                   Mat &bg_mat,
                                   Mat &fg_mat){
   //for measurement
    TimeStamp ts;
    Mat bg_warp_mat;
    Mat fg_warp_mat;
    assert(bg_pre.size() == bg_cur.size() && fg_pre.size() == fg_cur.size());
    bg_warp_mat = estimateRigidTransform(bg_pre, bg_cur, true);
    fg_warp_mat = estimateRigidTransform(fg_pre, fg_cur, true);
    bg_mat = bg_warp_mat;
    fg_mat = fg_warp_mat;
    this->m_disp_cost = TimeStamp::now()->diff(ts);
    return true;
}

static inline void combineRects(Rect x, Rect y, Rect &rst){
    Point2f tl(min(x.tl().x, y.tl().x), min(x.tl().y, y.tl().y));
    Point2f br(max(x.br().x, y.br().x), min(x.br().y, y.br().y));
    rst = Rect(tl, br);
}

bool MotionDetect::trackObject(Mat pre, TimeStamp pre_ts, Mat cur, TimeStamp cur_ts, Rect &area, Point2f &kernel, vector<Point2f> &obj_points){
    //for measurement
    TimeStamp ts;
    Mat pre_gray;
    Mat cur_gray;
    static int width;
    static int height;

    vector<Vec2f> disps;

    RotatedRect interest = minAreaRect(Mat(obj_points));
    Rect search_area;
    combineRects(area, interest.boundingRect(), search_area);
    assert(search_area.height <= cur.rows && search_area.width <= cur.cols);
    cvtColor(pre(search_area), pre_gray, CV_BGR2GRAY);
    cvtColor(cur(search_area), cur_gray, CV_BGR2GRAY);
    if(!this->m_old_flow.empty()){
        width = search_area.width;
        height = search_area.height;
    }

    calcOpticalFlowFarneback(pre_gray, cur_gray, this->m_old_flow, 0.5, 1, 40, 2, 7, 1.5, 0);

    int x = (int)kernel.x - area.x;
    int y = (int)kernel.y - area.y;

    for(int i = 0; i < (int)obj_points.size(); i++){
        Vec2f disp = this->m_old_flow.at<Vec2f>(obj_points[i].x - search_area.x, obj_points[i].y - search_area.y);
        disps.push_back(disp);
    }
    Scalar mean(0);
    Scalar stddev(0);
    meanStdDev(disps, mean, stddev);

    printf("x k = %f\t mean = %f\t stddev = %f\n",disps[0][0], mean[0], stddev[0]);
    printf("y k = %f\t mean = %f\t stddev = %f\n",disps[0][1], mean[1], stddev[1]);

    kernel.x += disps[0][0];
    kernel.y += disps[0][1];
    area = search_area;

    this->m_velo_x = this->m_velo_x * 0.5 + disps[0][0] / (cur_ts.diff(pre_ts));
    this->m_velo_y = this->m_velo_y * 0.5 + disps[0][1] / (cur_ts.diff(pre_ts));

    cout<<"velocity = " << m_velo_x <<"\t"<< m_velo_y <<"\n";
    this->m_old_ts = cur_ts;
    this->m_track_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool MotionDetect::spreadInterestPoint(cv::Mat pre, TimeStamp pre_ts, cv::Mat cur, TimeStamp cur_ts, Rect &area, Point2f& interest, vector<Point2f> &spreaded, bool use_old){
    //for measurement
    TimeStamp ts;
    Mat pre_gray;
    Mat cur_gray;
    static int width;
    static int height;
    cvtColor(pre(area), pre_gray, CV_BGR2GRAY);
    cvtColor(cur(area), cur_gray, CV_BGR2GRAY);
    if(use_old && !this->m_old_flow.empty()){
        calcOpticalFlowFarneback(pre_gray, cur_gray, this->m_old_flow, 0.5, 1, 40, 3, 5, 1.1, 0);
    }else{
        calcOpticalFlowFarneback(pre_gray, cur_gray, this->m_old_flow, 0.5, 1, 40, 3, 5, 1.1, 0);
        width = area.width;
        height = area.height;
    }


    int x = (int)interest.x - area.x;
    int y = (int)interest.y - area.y;
     cout<<"interest before = " << interest<<"\n";
    printf("area x = %d, y = %d width = %d height = %d\n", area.x, area.y, area.width, area.height);
    cout<<"tl = "<<area.tl()<<"\n";
    printf("x = %d, y = %d\n", x, y);

    if(x > area.width || x < 0)
        x = area.width / 2;
    if(y > area.height || y < 0)
        y = area.height / 2;

    float x_disp = this->m_old_flow.at<Vec2f>(y, x)[0];
    float y_disp = this->m_old_flow.at<Vec2f>(y, x)[1];

    printf("x_disp = %f, y_disp = %f\n", x_disp, y_disp);

    int max_disp = max(area.width, area.height);
    if(x_disp < -max_disp || x_disp > max_disp
            || y_disp < -max_disp || y_disp > max_disp)
        return false;

    interest.x += x_disp;
    interest.y += y_disp;
    cout<<"interest now = " <<interest<<"\n";
    printf("cols = %d, rows = %d\n", this->m_old_flow.cols, this->m_old_flow.rows);

    spreaded.clear();
    spreaded.push_back(interest);
    Mat mark = Mat::zeros(this->m_old_flow.size(), CV_8U);
    for(int i = 0; i < this->m_old_flow.rows; i++){
        for (int j = 0; j < this->m_old_flow.cols; j++){
            if(abs(this->m_old_flow.at<Vec2f>(i, j)[0] - x_disp) < 0.005
                    &&abs(this->m_old_flow.at<Vec2f>(i, j)[1] - y_disp) < 0.005){
                mark.at<unsigned char>(i, j) = 1;
                spreaded.push_back(Point2f(area.x + j + this->m_old_flow.at<Vec2f>(i, j)[0],
                                           area.y + i + this->m_old_flow.at<Vec2f>(i, j)[1]));
            }
        }
    }

    area.x = interest.x - width / 2;
    area.y = interest.y - height / 2;
    setRectRange(area, pre.cols, pre.rows);

    this->m_velo_x = x_disp / (cur_ts.diff(pre_ts));
    this->m_velo_y = y_disp / (cur_ts.diff(pre_ts));

    this->m_old_ts = cur_ts;
    this->m_calc_cost = TimeStamp::now()->diff(ts);
    return true;

}

bool MotionDetect::findInterestArea(Mat pre, Rect area, Mat cur, Mat &found, bool use_old_keypoints, int num){
   //for measurement
    TimeStamp ts;
    //for optical flow
    vector<Point2f> pre_points;
    vector<Point2f> cur_points;
    vector<unsigned char> status;
    vector<float> err;

    //for sift
    SIFT sift;
    vector<KeyPoint> keypoints;
    Mat gray;
    Mat mask;

    //for affine
    vector<Point2f> src;
    vector<Point2f> dst;
    Mat warp_mat;
    Mat temp;

    assert(num >= 3);
    cvtColor(pre, gray, CV_BGR2GRAY);
    mask = Mat::zeros(gray.rows, gray.cols, CV_8U);
    mask(area).setTo(1);
    if(use_old_keypoints && (int)this->m_old_points.size() >= 2 * num){
        for (int i = 0; i < (int)this->m_old_points.size(); i++){
            pre_points.push_back(this->m_old_points[i]);
        }
    }else{
        sift(gray, mask, keypoints, cv::noArray(), false);
        if ((int)keypoints.size() < num){
            return false;
        }
        for (int i = 0; i < (int)keypoints.size(); i++){
            pre_points.push_back(keypoints[i].pt);
        }
    }

    calcOpticalFlowPyrLK(pre, cur, pre_points, cur_points, status, err);

    for (int i = 0; i < (int)pre_points.size(); i++){
        if(status[i] > 0){
            dst.push_back(cur_points[i]);
            src.push_back(pre_points[i]);
        }
    }
    if((int)dst.size() < num){
        return false;
    }

    this->m_old_points = dst;

    warp_mat = estimateRigidTransform(src, dst, true);
    cout<<warp_mat;
    temp = Mat::zeros(cur.rows, cur.cols, cur.type());
    warpAffine(pre, temp, warp_mat, temp.size());
    temp(area).copyTo(found);

    this->m_find_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool MotionDetect::getBackgroundTransform(Mat pre, Rect fg_area, Mat cur, Mat &warp_mat, bool use_old_keypoints, int num){
   //for measurement
    TimeStamp ts;
    //for optical flow
    vector<Point2f> pre_points;
    vector<Point2f> cur_points;
    vector<unsigned char> status;
    vector<float> err;

    //for sift
    SIFT sift;
    vector<KeyPoint> keypoints;
    Mat gray;
    Mat mask;

    //for affine
    vector<Point2f> src;
    vector<Point2f> dst;

    assert(num >= 3);
    cvtColor(pre, gray, CV_BGR2GRAY);
    mask = Mat::zeros(gray.rows, gray.cols, CV_8U);
    mask(fg_area).setTo(1);
    if(use_old_keypoints && (int)this->m_old_points.size() >= 2 * num){
        for (int i = 0; i < (int)this->m_old_points.size(); i++){
            pre_points.push_back(this->m_old_points[i]);
        }
    }else{
        sift(gray, mask, keypoints, cv::noArray(), false);
        if ((int)keypoints.size() < num){
            return false;
        }
        for (int i = 0; i < (int)keypoints.size(); i++){
            pre_points.push_back(keypoints[i].pt);
        }
    }


    calcOpticalFlowPyrLK(pre, cur, pre_points, cur_points, status, err);

    for (int i = 0; i < (int)pre_points.size(); i++){
        if(status[i] > 0){
            dst.push_back(cur_points[i]);
            src.push_back(pre_points[i]);
        }
    }
    if((int)dst.size() < num){
        return false;
    }

    this->m_old_points = dst;

    warp_mat = estimateRigidTransform(src, dst, true);

    this->m_transform_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool MotionDetect::detectMovingObjects(Mat pre, Mat cur, Rect area, Mat &mask, int erode_iteration, int dilate_iteration){
   //for measurement
    TimeStamp ts;
    //for erode and dilate
    mask = Mat::zeros(cur.size(), CV_8U);
    this->m_bg_model(cur(area), mask(area), -1);
    erode(mask, mask, Mat(), Point(-1, -1), erode_iteration);
    dilate(mask, mask, Mat(), Point(-1, -1), dilate_iteration);
    this->m_detect_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool MotionDetect::shakeAdjust(Mat pre, Mat cur, Rect area, Rect obj_area, Mat &mask){
    //for measurement
    TimeStamp ts;
    mask = Mat::zeros(cur.size(), CV_8U);
    Mat temp = Mat::zeros(cur.size(), CV_8U);
    Mat cur_gray, pre_gray;
    vector<Point2f> cur_points, pre_points;
    vector<unsigned char> status;
    vector<float> err;
    cvtColor(cur, cur_gray, CV_BGR2GRAY);
    temp(area).setTo(1);
    temp(obj_area).setTo(0);
    goodFeaturesToTrack(cur_gray, cur_points, 16, 0.1, 3, temp);
    calcOpticalFlowPyrLK(cur, pre, cur_points, pre_points, status, err);

    vector<Point2f> src, dst;
    for(int i = 0; i < (int)cur_points.size(); i++){
        if(status[i] > 0){
            dst.push_back(cur_points[i]);
            src.push_back(pre_points[i]);
        }
    }

    if(src.size() < 3){
        fprintf(stderr, "shake lack points\n");
        return false;
    }
    Mat warp_mat = estimateRigidTransform(src, dst, true);
    Mat diff, ori_diff;

//    Mat pre_warp;
//    Mat cur_temp, pre_temp;


    cvtColor(pre, pre_gray, CV_BGR2GRAY);
    absdiff(cur_gray(area), pre_gray(area), ori_diff);
    warpAffine(pre, pre, warp_mat, pre.size());

//    cur.copyTo(cur_temp);
//    pre.copyTo(pre_temp);
//    for(int i = 0; i < 10 && i < (int)dst.size(); i++ ){
//        Scalar color(rand()&255, rand()&255, rand()&255);
//        circle(cur_temp, dst[i], 4, color, 3);
//        circle(pre_warp, dst[i], 4, color, 3);
//        circle(pre_temp, src[i], 4  , color, 3);
//    }

//    imshow("unwarpped", pre_temp);
//    imwrite("unwarpped.jpg", pre_temp);


//    imshow("warped", pre_warp);
//    imwrite("wrapped.jpg", pre_warp);

//    imwrite("cur.jpg", cur_temp);

    cvtColor(pre, pre_gray, CV_BGR2GRAY);
    absdiff(cur_gray(area), pre_gray(area), diff);
    Moments ori_diff_m, diff_m;
    ori_diff_m = moments(ori_diff, true);
    diff_m = moments(diff, true);
    double ref = 0;
    int count = 0;
    double moments00 = 0;
    printf("ori = %f  diff = %f ", ori_diff_m.m00, diff_m.m00);
    if(ori_diff_m.m00 < diff_m.m00){
        ori_diff.copyTo(diff);
        diff_m = ori_diff_m;
        for(int i = 0; i < (int)diff.rows; i++){
            for(int j = 0; j < (int)diff.cols; j++){
                if (diff.at<unsigned char>(i, j) > 0){
                    moments00 += diff.at<unsigned char>(i, j);
                    count++;
                }
            }
        }
        if(count != 0){
            ref = moments00 / count;
        }
    }else{
        ref = 30;
    }

    cout<<"ref = "<<ref<<"\n";
    for(int i = 0; i < (int)diff.rows; i++){
        for(int j = 0; j < (int)diff.cols; j++){
            if(diff.at<unsigned char>(i, j) > ref){
                mask(area).at<unsigned char>(i, j) = 255;
            }else{
                mask(area).at<unsigned char>(i, j) = 0;
            }
        }
    }
    erode(mask, mask, Mat());
    dilate(mask, mask, Mat());
    this->m_adjust_cost = TimeStamp::now()->diff(ts);
    return true;
}


bool MotionDetect::temperalDifferencing(Mat pre, Mat cur, Mat next, Rect area, Mat &mask, int threshold, int erode_iteration, int dilate_iteration){
    //for measurement
    TimeStamp ts;
    Mat pre_gray, cur_gray, next_gray, diff1, diff2, diff;
    cvtColor(pre, pre_gray, CV_BGR2GRAY);
    cvtColor(cur, cur_gray, CV_BGR2GRAY);
    cvtColor(next, next_gray, CV_BGR2GRAY);
    absdiff(cur_gray(area), pre_gray(area), diff1);
    absdiff(cur_gray(area), next_gray(area), diff2);
    mask = Mat::zeros(cur.size(), CV_8U);

//    Moments m = moments(diff, false);
    Moments m1 = moments(diff1);
    Moments m2 = moments(diff2);
    threshold = (int)(m1.m00 + m2.m00) / 2 / area.width / area.height;


    Mat lookUpTable(1, 256, CV_8U);
    uchar* p = lookUpTable.data;
    for( int i = 0; i < threshold + 1; ++i)
        p[i] = 0;
    for( int i = threshold + 1; i < 256; ++i){
        p[i] = 255;
    }


    LUT(diff1, lookUpTable, diff1);
    LUT(diff2, lookUpTable, diff2);

    bitwise_and(diff1, diff2, diff);

    mask(area).setTo(255, diff);
    erode(mask, mask, Mat(), Point(-1, -1), erode_iteration);
    dilate(mask, mask, Mat(), Point(-1, -1), dilate_iteration);
    this->m_adjust_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool MotionDetect::detectOpticalFlow(Mat pre, Mat cur, Rect area, Mat &mask, float threshold, int erode_iteration, int dilate_iteration){
    //for measurement
    TimeStamp ts;

    Mat pre_gray, cur_gray;


    cvtColor(pre, pre_gray, CV_BGR2GRAY);
    cvtColor(cur, cur_gray, CV_BGR2GRAY);

    calcOpticalFlowFarneback(pre_gray(area), cur_gray(area), this->m_old_flow, 0.5, 1, 10, 2, 7, 1.5, 0);

    Mat flow_mask = Mat::zeros(this->m_old_flow.size(), CV_8U);
    double xm00 = 0;
    double ym00 = 0;
    double ave_x;
    double ave_y;
    for(int i = 0; i < this->m_old_flow.rows; ++i){
        for( int j = 0; j < this->m_old_flow.cols; ++j){
            xm00 += abs(this->m_old_flow.at<Vec2f>(i, j)[0]);
            ym00 += abs(this->m_old_flow.at<Vec2f>(i, j)[1]);
        }
    }
    int num = this->m_old_flow.rows * this->m_old_flow.cols;
    ave_x = xm00 / num;
    ave_y = ym00 / num;
    for(int i = 0; i < this->m_old_flow.rows; ++i){
        for( int j = 0; j < this->m_old_flow.cols; ++j){
            float dx, dy;
            dx = abs(this->m_old_flow.at<Vec2f>(i, j)[0]);
            dy = abs(this->m_old_flow.at<Vec2f>(i, j)[1]);
            if(dx >   ave_x && dy >   ave_y){
                if(i + dy >=0 && j + dx >=0 && i + dy < this->m_old_flow.rows && j + dx < this->m_old_flow.cols);
                    flow_mask.at<unsigned char>(i , j ) = 255;
            }
        }
    }

    mask = Mat::zeros(cur.size(), CV_8U);
    fflush(stdout);
    mask(area).setTo(255, flow_mask);

    erode(mask, mask, Mat(), Point(-1, -1), erode_iteration);
    dilate(mask, mask, Mat(), Point(-1, -1), dilate_iteration);
    this->m_adjust_cost = TimeStamp::now()->diff(ts);

    return true;
}
